import math
import rclpy
from rclpy.node import Node
from general_interface.msg import ControlSignal


class Impulse(Node):
    def __init__(self):
        super().__init__("impulse")
        self.declare_parameter("topics", ["control_signal"])
        self.declare_parameter("rate", 100)
        self.topics = self.get_parameter("topics").value
        self.rate = self.get_parameter("rate").value

        self.pubs = []
        for topic in self.topics:
            self.pubs.append(self.create_publisher(ControlSignal, topic, 10))

        self.timer = self.create_timer(1.0 / self.rate, self.publish)

    def publish(self):
        msg = ControlSignal()
        msg.forward_control = (
            20.0
            / 180
            * math.pi
            * math.sin(
                self.get_clock().now().seconds_nanoseconds()[0] / math.pi * 2 / 5
            )
        )
        msg.rotate_control = 0.0
        for i in range(len(self.topics)):
            self.pubs[i].publish(msg)


def main(args=None):
    rclpy.init(args=args)
    node = Impulse()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()


if __name__ == "__main__":
    main()
